#!/usr/bin/env python
from __future__ import print_function

import math
import os
import sys
from collections import namedtuple

import numpy as np
import rospy
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import Point, PoseStamped, Twist
from nav_msgs.msg import Odometry

from util.angle import get_euler_orientation


def get_pose(message):
    global current_pose, current_twist
    current_pose = message.pose.pose
    # current_twist = message.twist[-1]

class master_trajectory_point():
    def __init__(self):
        self.target_vel_max = 4
        self.target_vel_small = 0.5
        self.pose = Point()
   
    def get_position_at(self, t):
        index = int(t) % 4
        if index == 0:  #top_right
            self.pose.x = 110
            self.pose.y = 0
        if index == 1:  #top_left
            self.pose.x = 110
            self.pose.y = 45
        if index == 2:  #low_left
            self.pose.x = -15
            self.pose.y = 45
        if index == 3:  #low_right
            self.pose.x = -15
            self.pose.y = 0

        return self.pose

# def angle_handle(angle):
#     if angle

if __name__ == '__main__':
    detal_index = 50
    sim_time = 10000
    DELTA_T = 0.02
    traj = master_trajectory_point()
    rospy.init_node('control')
    # current_pose = None
    # current_twist = None"/nexus0/base_pose_ground_truth", 10,
    subscriber = rospy.Subscriber('/nexus0/base_pose_ground_truth', Odometry, get_pose)
    # subscriber = rospy.Subscriber('gazebo/model_states', ModelStates, get_pose)
    # twist_publisher = rospy.Publisher('computed_control_actions',
    #                                   Twist,
    #                                   queue_size=1)
    publisher = rospy.Publisher('/nexus0/cmd_vel', Twist, queue_size=1)

    current_pose = None
    # current_twist = None


    STEPS = sim_time
    rate = rospy.Rate(int(1 / DELTA_T))
    t = 0
    point_t = 0
    theta_exp = 0
    point_last = traj.get_position_at(point_t)
    inter_delta_theta = 0 
    while not rospy.is_shutdown() : #and t < STEPS:
        if current_pose is None :
            continue
        pose = traj.get_position_at(point_t)
        delta_x = pose.x - current_pose.position.x
        delta_y = pose.y - current_pose.position.y
        delta_x_last = point_last.x - current_pose.position.x
        delta_y_last = point_last.y - current_pose.position.y
        detal_dis = math.sqrt(( delta_x )**2 + (delta_y)**2)
        delta_dis_last = math.sqrt(delta_x_last**2 + delta_y_last**2)
        # detal_dis_contrl = delta_x + delta_y
        theta_state = get_euler_orientation(current_pose.orientation)[2]
        theta_ponit_exp = math.atan2(delta_y, delta_x)
        if point_t == 2:
            if theta_state < 0:
                theta_state = theta_state +2*math.pi 
            if theta_ponit_exp < 0:
                theta_ponit_exp = theta_ponit_exp +2*math.pi 
        if point_t == 3:
            if theta_state > 0:
                theta_state = theta_state - 2*math.pi 
            # if theta_ponit_exp < 0:
            #     theta_ponit_exp = theta_ponit_exp +2*math.pi 

        delta_theta= theta_ponit_exp - theta_state
        # delta_theta= theta_exp - theta_state
        print("oooooooo theta_state", theta_state)
        print("delta_y", delta_y)
        print("delta_x", delta_x)
        print("theta_ponit_exp", theta_ponit_exp)
        print("datal_theata", delta_theta)
        print("detal_dis", detal_dis)
        print("delta_dis_last", delta_dis_last)
        print("pose", pose)
        print("robot state", current_pose)
        vel_x = detal_dis*1
        # inter_delta_theta = inter_delta_theta + delta_theta
        w_z = delta_theta*0.4
        if  abs(delta_theta) < 0.05:
            w_z = delta_theta*0.5

        if w_z > 0.5:
            w_z = 0.5
        if w_z <-0.5:
            w_z = -0.5
        if detal_dis >20:
            if vel_x > 4.0:
                vel_x = 4.0
        elif detal_dis <=20 and detal_dis > 10:
            if vel_x > 1:
                vel_x = 2

        if current_pose.position.x > 105 and current_pose.position.x < 114 and current_pose.position.y > -4.5 and current_pose.position.y <4.5:
            # theta_exp = math.pi/2
            point_t = 1
        if current_pose.position.x > 105 and current_pose.position.x < 114  and current_pose.position.y > 40.5 and current_pose.position.y <49.5:
            # theta_exp = 2*math.pi/2
            point_t = 2 
        if current_pose.position.x > -20 and current_pose.position.x < -10  and current_pose.position.y > 40.5 and current_pose.position.y < 49.5:
            # theta_exp = 3*math.pi/2
            point_t = 3
        if current_pose.position.x > -20 and current_pose.position.x < -10  and current_pose.position.y > -4.5 and current_pose.position.y <4.5:
            # theta_exp = 0
            point_t = 0
        
        pose = traj.get_position_at(point_t)
        theta_state = get_euler_orientation(current_pose.orientation)[2]
        theta_ponit_exp = math.atan2(delta_y, delta_x)
        delta_theta= theta_ponit_exp - theta_state
        print("fffffffffffff  delta theta", delta_theta)
        if current_pose.position.x > 105 and current_pose.position.x < 114 and current_pose.position.y > -4.5 and current_pose.position.y <4.5:
            #if abs(delta_theta) > 0.03:
                vel_x = 2
                w_z = 0.4
        if current_pose.position.x > 105 and current_pose.position.x < 114  and current_pose.position.y > 40.5 and current_pose.position.y <49.5:
            #if abs(delta_theta) > 0.03:
                vel_x = 2
                w_z = 0.4
        if current_pose.position.x > -20 and current_pose.position.x < -10  and current_pose.position.y > 40.5 and current_pose.position.y < 49.5:
            #if abs(delta_theta) > 0.03:
                vel_x = 2
                w_z = 0.4
        if current_pose.position.x > -20 and current_pose.position.x < -10  and current_pose.position.y > -4.5 and current_pose.position.y <4.5:
            #if abs(delta_theta) > 0.03:
                vel_x = 2
                w_z = 0.4

        print("vel_x", vel_x)
        print("w_z", w_z)
        twist = Twist()
        twist.linear.x = vel_x
        twist.angular.z = w_z
        publisher.publish(twist)

        t += 1
        rate.sleep()
    rospy.spin()()
